package kit.edu.lego.gruppe3.levels.labyrinth;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import lejos.nxt.LCD;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class MoveParallelToWall implements Behavior{
	
	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();
	
	@Override
	public void action()
	{
		LCD.drawString("MPW", 0,1);
		robot.setTravelSpeed(15);
		robot.forward();
		
	}

	@Override
	public void suppress()
	{
	}

	@Override
	public boolean takeControl()
	{
		LCD.drawString("Position "+	ComponentsRepository.getInstance().getMiddleMotor().getPosition(), 0,6);
		return LevelSelector.getInstance().isLabyrinth() &&
			ComponentsRepository.getInstance().getSonicSensor().getDistance() < 25;
	}
	
	

}
